/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.impulserobotics;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Joystick.ButtonType;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Relay;


/**
 *
 * @author Sahil Jain
 */
public class CameraMover {

    public Servo horizontalRotator;
    public Servo verticalRotator;
    double hcurAngle;
    double vcurAngle;
    double vSpeed;
    double hSpeed;
    boolean shootingMode = false;
    Relay r = new Relay(2);
    Timer time = new Timer();
   // public Shooter shoot = new Shooter();
    /**
     * 
     * @param hServoPWM Horizontal Rotation Servo PWM Connection port.
     * @param vServoPWM Vertical Rotation Servo PWM Connection port.
     */
    public CameraMover(int hServoPWM, int vServoPWM) {
        horizontalRotator = new Servo(hServoPWM);
        verticalRotator = new Servo(vServoPWM);
        hcurAngle = horizontalRotator.getAngle();
        vcurAngle = verticalRotator.getAngle();
    }

    /**
     * 
     * @param inJoy The joystick that will control the camera
     */
    public void move(Joystick inJoy) {   //shooting
        //if ((inJoy.getButton(ButtonType.kTrigger))&&(inJoy.getRawButton(Constants.AIM_BUTTON))){
        //     shoot.fire(true);
                          
        //}
        if (inJoy.getRawButton(Constants.SERVO_CENTRE_BUTTON)) {
            centre();
        } else {
            SmartDashboard.putDouble("hcurAngle", hcurAngle);
             SmartDashboard.putDouble("vcurAngle", vcurAngle);
            // horizontalRotator.setAngle(inJoy.getX()*90+90);
            // verticalRotator.setAngle(inJoy.getY()*90+90);
            // horizontalRotator.setAngle(horizontalRotator.getAngle()+inJoy.getX()*3);
            // verticalRotator.setAngle(verticalRotator.getAngle()+inJoy.getY()*3);
            
             hSpeed = inJoy.getX() * 4;
            vSpeed = inJoy.getY() * 4;
            if(hSpeed < 0.1 && hSpeed > -0.1){
                hSpeed = 0;
            }
            if(vSpeed < 0.1 && vSpeed > -0.1){
                vSpeed = 0;
            }
            if (hcurAngle > 180)
                hcurAngle = 180;
            if (hcurAngle < 0)
                hcurAngle = 0;
            if (vcurAngle > 180)
                vcurAngle = 180;
            if (vcurAngle < 0)
                vcurAngle = 0;
           
            vcurAngle += vSpeed;
            if(shootingMode){
                horizontalRotator.setAngle(90);
            }else{
                 hcurAngle += hSpeed;
            horizontalRotator.setAngle(hcurAngle);
            }
            verticalRotator.setAngle(vcurAngle);
        }
    }

    public void centre() {
        horizontalRotator.setAngle(90);
        verticalRotator.setAngle(10);
        hcurAngle = 90;
        vcurAngle = 10;
    }

    void setShootingMode(boolean in) {
        shootingMode = in;
        
    }
}
